//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2005
//
// Distance over sphere.
//

# include "StdAfx.h"
# include "Distance.h"
# include "PtAlgo.h"
# include "Globals.h"

const double c_A = 6378137;
const double c_a = 1/298.257223563;
const double c_e2 = 2*c_a - c_a*c_a;

float Distance (const point_t & _1, const point_t & _2) {
	const double fSinB1 = ::sin (_1.y*c_PI/180);
	const double fCosB1 = ::cos (_1.y*c_PI/180);
	const double fSinL1 = ::sin (_1.x*c_PI/180);
	const double fCosL1 = ::cos (_1.x*c_PI/180);

	const double N1 = c_A/::sqrt (1 - c_e2*fSinB1*fSinB1);

	const double X1 = N1*fCosB1*fCosL1;
	const double Y1 = N1*fCosB1*fSinL1;
	const double Z1 = (1 - c_e2)*N1*fSinB1;

	const double fSinB2 = ::sin (_2.y*c_PI/180);
	const double fCosB2 = ::cos (_2.y*c_PI/180);
	const double fSinL2 = ::sin (_2.x*c_PI/180);
	const double fCosL2 = ::cos (_2.x*c_PI/180);

	const double N2 = c_A/::sqrt (1 - c_e2*fSinB2*fSinB2);

	const double X2 = N2*fCosB2*fCosL2;
	const double Y2 = N2*fCosB2*fSinL2;
	const double Z2 = (1 - c_e2)*N2*fSinB2;

	const double D = ::sqrt ((X1 - X2)*(X1 - X2) + (Y1 - Y2)*(Y1 - Y2) + (Z1 - Z2)*(Z1 - Z2));

	const double R = N1;
	const double D2 = 2*R*::asin (.5f*D/R);

	return D2;
}

# ifdef _DEBUG
void Test_Distance () {
	// MapSource  156,10 km
	// OZI  159,07
	point_t pt1 (36.91672f, 55.73837f);
	point_t pt2 (37.35804f, 57.14513f);
//	point_t pt1 (-90, 0);
//	point_t pt2 (90, 0);
	Distance (pt1, pt2);
}
# endif // _DEBUG
